#include "LegacyMotionConverter.h"

#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensorMeasurement.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensorMeasurement.h>

#include <algorithm>

using namespace MMM;

LegacyMotionConverter::LegacyMotionConverter(const std::string &motionFilePath) : motionFilePath(motionFilePath)
{
    RapidXMLReaderPtr reader = RapidXMLReader::FromFile(motionFilePath);
    RapidXMLReaderNodePtr root = reader->getRoot();
    if (root && root->name() == XML::MOTION_XML_ROOT && root->has_attribute(XML::ATTRIBUTE_VERSION) && root->attribute_value(XML::ATTRIBUTE_VERSION) == "2.0")
        throw Exception::XMLFormatException("Please don't try to convert non legacy motions!");

    LegacyMotionReaderXMLPtr legacyMotionReader(new LegacyMotionReaderXML());
    legacyMotions = legacyMotionReader->loadAllMotions(motionFilePath);
}

MotionList LegacyMotionConverter::convertMotions() {
    MotionList motions;
    std::set<std::string> motionNames;
    for (auto legacyMotion : legacyMotions) {
        if (legacyMotion) {
            try {
                MotionPtr motion = convertMotion(legacyMotion);
                if (motionNames.find(motion->getName()) != motionNames.end()) throw MMM::Exception::MMMException("Motion name '" + motion->getName() + "' is not unique!");
                motionNames.insert(motion->getName());
                motions.push_back(motion);
            }
            catch (Exception::MMMException &e) {
                MMM_WARNING << "Skipping legacy motion " << legacyMotion->getName() << ". " << e.what() << std::endl;
            }
        }
    }
    return motions;
}

MotionList LegacyMotionConverter::convertMotions(std::vector<MotionListConfigurationPtr> configurations) {
    std::sort(configurations.begin(), configurations.end(), SortByNewIndexFunctor());
    MotionList motions;
    std::set<std::string> motionNames;
    for (auto configuration : configurations) {
        if (!configuration->isIgnoreMotion()) {
            LegacyMotionPtr legacyMotion = legacyMotions.at(configuration->getIndex());
            legacyMotion->setName(configuration->getMotionName());
            MotionPtr motion = convertMotion(legacyMotion);
            if (motionNames.find(motion->getName()) != motionNames.end()) throw MMM::Exception::MMMException("Motion name '" + motion->getName() + "' is not unique!");
            motionNames.insert(motion->getName());
            motions.push_back(motion);
        }
    }
    return motions;
}

MotionPtr LegacyMotionConverter::convertMotion(LegacyMotionPtr legacyMotion) {
    std::string motionName = legacyMotion->getName();
    if (motionName.empty()) throw MMM::Exception::MMMException("Motion name is empty!");
    else if (!legacyMotion->getModel()) throw MMM::Exception::MMMException("No model in motion '" + legacyMotion->getName() + "'.");

    MotionPtr motion(new Motion(motionName, legacyMotion->getModel(false), legacyMotion->getModel(), legacyMotion->getModelProcessor(), motionFilePath));

    KinematicSensorPtr kinematicSensor(new KinematicSensor(legacyMotion->getJointNames()));
    ModelPoseSensorPtr modelPoseSensor(new ModelPoseSensor());

    for (auto motionFrame : legacyMotion->getMotionFrames()) {
        KinematicSensorMeasurementPtr kMeasurement(new KinematicSensorMeasurement(motionFrame->timestep, motionFrame->joint));
        kinematicSensor->addSensorMeasurement(kMeasurement);
        ModelPoseSensorMeasurementPtr mpMeasurement(new ModelPoseSensorMeasurement(motionFrame->timestep, motionFrame->getRootPos(), motionFrame->getRootRot()));
        modelPoseSensor->addSensorMeasurement(mpMeasurement);
    }

    // exceptions will be forwared
    if (kinematicSensor->getJointNames().size() > 0) motion->addSensor(kinematicSensor);
    motion->addSensor(modelPoseSensor);

    return motion;
}

std::vector<std::string> LegacyMotionConverter::getMotionNames() {
    std::vector<std::string> motionNames;
    for (auto legacyMotion : legacyMotions) motionNames.push_back(legacyMotion->getName());
    return motionNames;
}

LegacyMotionList LegacyMotionConverter::getMotionsWithoutModel() {
   LegacyMotionList legacyMotionsWithoutModel;
   for (auto legacyMotion : legacyMotions) {
       if(!legacyMotion->getModel()) legacyMotionsWithoutModel.push_back(legacyMotion);
   }
   return legacyMotionsWithoutModel;
}
